/*
Code to get keyboard inputs without blocking the loop and without pressing enter
*/

#include <ros/ros.h>
#include <termios.h>
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
// %EndTag(MSG_HEADER)%

#include <sstream>

int getch()
{
  static struct termios oldt, newt;
  tcgetattr( STDIN_FILENO, &oldt);           // save old settings
  newt = oldt;
  newt.c_lflag &= ~(ICANON);                 // disable buffering      

  newt.c_cc[VMIN] = 0; 
  newt.c_cc[VTIME] = 1;

  tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

  int c = getchar();  // read character (non-blocking)

  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
  return c;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "key_input_node");
	ros::NodeHandle n;
    // %Tag(PUBLISHER)%
    ros::Publisher cmd_pub = n.advertise<geometry_msgs::Twist>("chatter", 1000);

	ros::Rate loop_rate(1000);

    int c = 0 ,shadow=0;
	int ii=0,jj=0;
    geometry_msgs::Twist cmd;

	while (ros::ok())
	{		
		c=getch();

		if(c==-1)
		 jj=500;
        else
		{
			jj=10;
		}

        if(c!=shadow)
        {
           switch(c)
           {
           case 'i':cmd.linear.x = 1;break;        //up
           case 'j':cmd.linear.x = 2;break;        //left
           case 'm':cmd.linear.x = 3;break;        //down
           case 'l':cmd.linear.x = 4;break;        //right

           case 'c':cmd.linear.x = 0;cmd.linear.y = 1;break;        //this is used for check xunfei voice angle

           default:cmd.linear.x = 5;cmd.linear.y = 0;         //stop
           }

           cmd_pub.publish(cmd);
        }

        shadow=c;
        //if(c!=-1)
        //ROS_INFO("%c", c);

		ros::spinOnce();
		
		for(ii=0;ii<jj;ii++)
		loop_rate.sleep();
	}

	return 0;
}
